Laser&Odometry Extrinsic Calibration

问题描述:

如图所示,2d机器人在世界系下从k位置运动到k+1位置,机器人body坐标系位姿分别为$T_{bk}^w$和$T_{bk+1}^w$,body坐标系和sensor坐标系得相对运动分别为$T_{bk+1}^{bk}$和$T_{lk+1}^{lk}$。需要求解sensor坐标系和body坐标系之间的外参$T_l^b$。

误差函数:

对于sensor相对位姿:

对于body相对位姿:

带入上式得:

由此等式,$T_{lk+1}^{lk}$可以通过激光ICP得到观测值,$T_{bk+1}^{bk}$可以通过里程计累计得到预测值。因此可以定义误差函数:

code

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
template <class T> Eigen::Matrix<T, 3, 1> inverse(const Eigen::Matrix<T, 3, 1>& value)
{
Eigen::Matrix<T, 3, 1> result;
result[0] = -value[0] * ceres::cos(value[2]) - value[1] * ceres::sin(value[2]);
result[1] = value[0] * ceres::sin(value[2]) - value[1] * ceres::cos(value[2]);
result[2] = -value[2];
return result;
}
template <class T> Eigen::Matrix<T, 3, 1> plus(const Eigen::Matrix<T, 3, 1>& left, const Eigen::Matrix<T, 3, 1>& right)
{
Eigen::Matrix<T, 3, 1> result;
result[0] = left[0] + right[0] * ceres::cos(left[2]) - right[1] * ceres::sin(left[2]);
result[1] = left[1] + right[0] * ceres::sin(left[2]) + right[1] * ceres::cos(left[2]);
result[2] = left[2] + right[2];
return result;
}

struct CostFunctor {
CostFunctor(const Eigen::Vector3d& tbkbkp, const Eigen::Vector3d& tlklkp)
: tbkbkp_(tbkbkp), tlklkp_(tlklkp)
{}
template <typename T>
bool operator()(const T* const parameters, T* residual) const {
Eigen::Matrix<T, 3, 1> tbl(parameters[0], parameters[1], parameters[2]);
Eigen::Matrix<T, 3, 1> tbkbkp(T(tbkbkp_(0)), T(tbkbkp_(1)), T(tbkbkp_(2)));
Eigen::Matrix<T, 3, 1> tlklkp = plus(plus(inverse(tbl), tbkbkp), tbl);
//Eigen::Matrix<T, 3, 1> tlklkp = plus(tbkbkp, tbl);
residual[0] = T(tlklkp_(0)) - tlklkp(0);
residual[1] = T(tlklkp_(1)) - tlklkp(1);
residual[2] = T(tlklkp_(2)) - tlklkp(2);

//std::cout << "tbl:" << std::endl << tbl << std::endl;
//std::cout << "residual:" << std::endl << residual[0] << std::endl << residual[1] << std::endl << residual[2] << std::endl;
//std::cout << "tbkbkp:" << std::endl << tbkbkp << std::endl;
//std::cout << "tlklkp:" << std::endl << tlklkp << std::endl;
return true;
}
Eigen::Vector3d tbkbkp_, tlklkp_;
};

// main
Eigen::Vector3d tbl = Eigen::Vector3d(1., 0., -1.4 * M_PI / 2.);
Eigen::Vector3d tlbe = Eigen::Vector3d(100., 100., 0.);
//Eigen::Vector3d tlbe = tbl;
ceres::Problem problem;
Eigen::Vector3d initial = Eigen::Vector3d::Zero();
Eigen::Vector3d groundtruth = initial;
for (auto pos = 0; pos < 10; pos++) {
Eigen::Vector3d twbk = groundtruth + Eigen::Vector3d::Random() * 0.0;
Eigen::Vector3d twlk = plus(groundtruth, tbl);
twlk += Eigen::Vector3d::Random() * 0.0;
std::cout << "twbk: " << twbk.transpose() << " twlk: " << twlk.transpose() << std::endl;

Eigen::Vector3d tbkbkp = plus(inverse(initial), twbk);
Eigen::Vector3d tlklkp = plus(inverse(plus(initial, tbl)), twlk);
std::cout << "tbkbkp: " << tbkbkp.transpose() << " tlklkp: " << tlklkp.transpose() << std::endl;
ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<CostFunctor, 3, 3>(new CostFunctor(tbkbkp, tlklkp));
//ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<CostFunctor, 3, 3>(new CostFunctor(twbk, twlk));
problem.AddResidualBlock(cost_function, nullptr, tlbe.data());

groundtruth = groundtruth + Eigen::Vector3d(1., 1., 0.) + Eigen::Vector3d::Random()/**/;
}
ceres::Solver::Options options;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

std::cout << summary.FullReport() << "\n";
std::cout << "truth: " << tbl.transpose() << " " << "estimate: " << tlbe.transpose() << std::endl;

Reference:

[1] Censi, Andrea, Franchi, Antonio, Marchionni, Luca & Oriolo, Giuseppe (2013). Simultaneous calibration of odometry and sensor parameters for mobile robots. IEEE Transactions on Robotics, 29, 475-492
[2] Robot sensor calibration: solving AX=XB on the Euclidean group
[3] https://github.com/AndreaCensi/calibration.git